from global_var import *
import global_var
from All_Init import *
import All_Init
from Test import *
from UI import *
from Image import *
import Image
from machine import *
from PID import *
from Myfilter import *
from machine import WDT
import Myfilter
import gc
import time
M_PI = 3.1415926
Target_Mid = 0
err = 0
Real_speed = 0
Time_count = 0
def IMU_Offset(mode):
    if mode == 1:
        OFFSET_COUNT = 500
        temp = [0] * 3
        for i in range(OFFSET_COUNT):
            if(ticker_flag): 
                global_var.imu_data = All_Init.imu.get()
                Gyro_Process()
                temp[0] += (global_var.imu_data[3])
                temp[1] += (global_var.imu_data[4])
                temp[2] += (global_var.imu_data[5])
            time.sleep_ms(2) 
        global_var.Gyro['gx_bias']= temp[0] / OFFSET_COUNT
        global_var.Gyro['gy_bias'] = temp[1] / OFFSET_COUNT
        global_var.Gyro['gz_bias'] = temp[2] / OFFSET_COUNT
    elif mode == 2:
        global_var.Gyro['gx_bias'] = 3.27
        global_var.Gyro['gy_bias'] = -4.34
        global_var.Gyro['gz_bias'] = -6.03
def Gyro_Process():
    global_var.Gyro['ax'] = global_var.imu_data[0]
    global_var.Gyro['ay'] = global_var.imu_data[1]
    global_var.Gyro['az'] = global_var.imu_data[2]
    global_var.Gyro['gx'] = ((float)(global_var.imu_data[3]) - global_var.Gyro['gx_bias'])
    global_var.Gyro['gy'] = ((float)(global_var.imu_data[4]) - global_var.Gyro['gy_bias'])
    global_var.Gyro['gz'] = ((float)(global_var.imu_data[5]) - global_var.Gyro['gz_bias'])
def time_pit_handler(time):#5
    global Time_count,err,L_dif,R_dif
    global_var.imu_data = All_Init.imu.get()
    Gyro_Process()
    global_var.Cal_Angle = angle_calc(global_var.Gyro['ay'], global_var.Gyro['gx'])
    AccRing_Input = (-global_var.Gyro['gx']) + Angle_PID.Output
    Acc_PID.Loc_PID(AccRing_Input)
    Acc_Turn_PID.Loc_PID(-Turn_PID.Output - global_var.Gyro['gz'])
    if(Image.Ramp_State >= 2):
        Acc_PID.Output *= 0.6
        Acc_Turn_PID.Output *= 0.6
        if(Image.Ramp_finish_flag == 1):
            Acc_PID.Output *= 0.8
            Acc_Turn_PID.Output *= 0.8
    if(global_var.Car_Go == 1 and global_var.Stop_start == 0):
        is_right_turn = Acc_Turn_PID.Output >= 0
        if Image.Slow_State == 2:
            outside_dif = global_var.Outside_dif if is_right_turn else global_var.Inside_dif
            inside_dif = global_var.Inside_dif if is_right_turn else global_var.Outside_dif
        else:
            outside_dif = -global_var.R_dif if is_right_turn else global_var.L_dif
            inside_dif = global_var.R_dif if is_right_turn else -global_var.L_dif
        PWM_L = int(Acc_PID.Output + Acc_Turn_PID.Output * (1 - outside_dif))
        PWM_R = int(Acc_PID.Output - Acc_Turn_PID.Output * (1 - inside_dif))
        motor_control(1, -PWM_R)
        motor_control(2, PWM_L)
    if(global_var.Test_Go == 1 and global_var.Stop_start == 0):
        PWM_L = 2000
        PWM_R = 2000
        motor_control(2, PWM_L)
        motor_control(1, -PWM_R)
count_time50 = 0
speed_flag = 0
def time_pit_handler2(time):#10
    global ticker_flag,err,Target_Mid,Real_speed,speed_flag
    global aim_speed,count_time50,Real_speed,Time_count
    ticker_flag = True
    count_time50 += 1
    if(count_time50 == 10):
        if((global_var.Car_Go == 1 ) or global_var.Test_Go == 1 ):
            Real_speed = Encoder_filter()
            if (abs(Real_speed) > int(global_var.Param_dict["Realim"]) * 100 or
            global_var.Speed_L > int(global_var.Param_dict["L_lim"]) * 100 or global_var.Speed_R > int(global_var.Param_dict["R_lim"]) * 100 or
                 (Image.ccd3_ave_threshold <= int(global_var.Param_dict["outave"]) and Image.c3_cbh_thr <= int(global_var.Param_dict["outcbh"]))):
                global_var.Stop_start = 1
            Speed_PID.Speed_Loc_PID(Image.aim_speed-Real_speed)
        count_time50 = 0
    if Image.Ramp_State >= 2 or Image.green_task != 0:
        Speed_PID.Output = 0
    if Image.Ramp_slow_flag == 1:
        Speed_PID.Output = 100
    if Image.Slow_State == 1:
        Speed_PID.Output = 100
    Target_Mid = Speed_PID.Output + global_var.Mid_Angle
    AngleRing_Input = Target_Mid - global_var.Cal_Angle
    Angle_PID.Loc_PID(AngleRing_Input)
    Turn_PID.Loc_Turn_PID(err,Image.aim_speed,Real_speed,Dynamic_flag = 1)
def Param_Init():
    for name in global_var.Param_names:
        global_var.Param_dict[name] = 0
    global_var.keys_list = sorted(global_var.Param_dict.keys(), key=lambda x: Param_names.index(x))
    value_length = len(global_var.keys_list)
    for k in range(value_length):
        global_var.Param_dict[global_var.keys_list[k]] = global_var.Param_lst[k]
    global_var.Crvspeed =  global_var.Param_dict["speed"]
    global_var.Mid_Angle =  global_var.Param_dict["Mid"]
    global_var.c1_cbh =  global_var.Param_dict["c1_cbh"]
    global_var.c2_cbh =  global_var.Param_dict["c2_cbh"]
    global_var.c3_cbh =  global_var.Param_dict["c3_cbh"]
    global_var.c1_avth =  global_var.Param_dict["c1_avth"]
    global_var.c2_avth =  global_var.Param_dict["c2_avth"]
    global_var.c3_avth =  global_var.Param_dict["c3_avth"]
    global_var.L_dif = global_var.Param_dict["L_dif"]
    global_var.R_dif = global_var.Param_dict["R_dif"]
    global_var.Outside_dif = global_var.Param_dict["SloOut"]
    global_var.Inside_dif = global_var.Param_dict["SloIn"]
    global_var.Mode = int(global_var.Param_dict["mode"])
pit1 = ticker(1)
pit1.capture_list(All_Init.imu)
pit1.callback(time_pit_handler)
pit1.start(5)
ccd = TSL1401(1)
ticker_flag = False
pit2 = ticker(2)
pit2.capture_list(All_Init.encoder_l, All_Init.encoder_r,ccd)
pit2.callback(time_pit_handler2)
pit2.start(10)
led1 = Pin('C4' , Pin.OUT, pull = Pin.PULL_UP_47K, value = True)
end_switch = Pin('C19', Pin.IN, pull=Pin.PULL_UP_47K, value = True)
end_state = end_switch.value()
beep = Pin('D24' , Pin.OUT, pull = Pin.PULL_UP_47K, value = False)
IMU_Offset(2)
Param_Init()
PID_Init()
MODE()
wdt = WDT(timeout=200)
while True:
#     encoder_Data()
    if (ticker_flag):
        global_var.ccd_data1 = ccd.get(0)
        global_var.ccd_data2 = ccd.get(1)
        global_var.ccd_data3 = ccd.get(2)
        user_data1 = list(global_var.ccd_data1)
        user_data2 = list(global_var.ccd_data2)
        user_data3 = list(global_var.ccd_data3)
        err = Image_Process(user_data1,user_data2,user_data3,Real_speed)
        ticker_flag = False
    if((Image.red_flag == 1 or Image.Island_Judge == 1 or Image.Island_State == 3 or Image.Island_State == 5
        or (Image.Zebra_3_flag == 1 and global_var.Car_Go == 1) or Image.Ramp_State != 0 or Image.green_task != 0 or Image.block_state != 0 or Image.Slow_flag == 1)
        and int(global_var.Param_dict["beep"]) != 0):
        beep.value(1)
    else:
        beep.value(0)
    wireless.send_oscilloscope(Image.red_flag,Image.Island_Judge,Image.Island_State,Image.count,Image.check_flag,Image.ccd2_left_index,Image.ccd2_right_index)
#     wireless.send_oscilloscope(Image.block_num,Image.ccd1_left_index,Image.ccd1_right_index,Image.ccd2_left_index,Image.ccd2_right_index,Image.ccd3_left_index,Image.ccd3_right_index,Image.block_state)
#     wireless.send_oscilloscope(Image.ccd1_error,Image.ccd1_left_index,Image.ccd1_right_index,Image.ccd2_left_index,Image.ccd2_right_index,Image.ccd3_left_index,Image.ccd3_right_index,Image.block_state)
#     wireless.send_oscilloscope(Image.Ramp_State,Image.Ramp_count,Image.Ramp_finish_flag,Image.ac_ccd3_road_len,Image.aim_speed,Real_speed,global_var.Gyro['az'],global_var.Stop_start)
#     wireless.send_oscilloscope(global_var.Speed_L,global_var.Speed_R,Real_speed)
    if(global_var.Car_Go == 0 ):
        Button_Event()
        UI_Main()
    if(global_var.Stop_start == 1):
        motor_control(1, 0)
        motor_control(2, 0)
        lcd.str16(50, 50, "safe",0xffff)
    gc.collect()
    wdt.feed()
    if end_switch.value() != end_state:
        print("Ticker stop.")
        break